/* Copyright (c) <2003-2022> <Newton Game Dynamics>
* 
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* 
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/

#include "ndSandboxStdafx.h"
#include "ndPhysicsWorld.h"
#include "ndDemoEntityNotify.h"

ndDemoEntityNotify::ndDemoEntityNotify(ndDemoEntityManager* const manager, const ndSharedPtr<ndRenderSceneNode>& entity, ndBodyKinematic* const parentBody, const ndVector& gravity)
	:ndModelBodyNotify(parentBody, gravity)
	,m_manager(manager)
	,m_entity(entity)
	,m_transform()
	,m_capSpeed(ndFloat32 (100.0f))
{
	if (!parentBody)
	{
		// handle root level and intances single bodies
		ndRenderSceneNode* const parent = entity->GetParent();
		const ndMatrix parentMatrix(parent ? parent->CalculateGlobalTransform() : ndGetIdentityMatrix());
		m_bindMatrix = parentMatrix.OrthoInverse();
	}
	else
	{
		// handle hierarchies of rigid nodies 
		ndDemoEntityNotify* const notify = (ndDemoEntityNotify*)parentBody->GetNotifyCallback();
		ndSharedPtr<ndRenderSceneNode> parentEntity (notify->GetUserData());

		ndMatrix matrix(ndGetIdentityMatrix());
		for (const ndRenderSceneNode* parent = entity->GetParent(); parent != *parentEntity; parent = parent->GetParent())
		{
			const ndMatrix parentMatrix(parent->GetTransform().GetMatrix());
			matrix = matrix * parentMatrix;
		}
		m_bindMatrix = matrix;
	}
}

ndDemoEntityNotify::ndDemoEntityNotify(const ndDemoEntityNotify& notify)
	:ndModelBodyNotify(notify)
	,m_manager(notify.m_manager)
	,m_entity(notify.m_entity)
	,m_transform()
	,m_capSpeed(100.0f)
{
	if (notify.GetParentBody())
	{
		ndAssert(0);
	}
	else
	{
		ndAssert(0);
	}
}

ndDemoEntityNotify::~ndDemoEntityNotify()
{
}

void ndDemoEntityNotify::OnApplyExternalForce(ndInt32 threadIndex, ndFloat32 timestep)
{
	ndModelBodyNotify::OnApplyExternalForce(threadIndex, timestep);

	ndBodyKinematic* const body = GetBody()->GetAsBodyKinematic();
	if (body->GetInvMass() > 0.0f)
	{
		// Clamp huge angular and linear velocities generated by high speed collisions
		// or stiff joints loops
		ndVector omega(body->GetOmega());
		ndVector veloc(body->GetVelocity());
		ndFloat32 omega2(omega.DotProduct(omega).GetScalar());
		if (omega2 > m_capSpeed * m_capSpeed)
		{
			omega = omega.Normalize().Scale(m_capSpeed);
			body->SetOmega(omega);
		}

		ndFloat32 veloc2(veloc.DotProduct(veloc).GetScalar());
		if (veloc2 > m_capSpeed * m_capSpeed)
		{
			veloc = veloc.Normalize().Scale(m_capSpeed);
			body->SetVelocity(veloc);
		}
	}
}

void ndDemoEntityNotify::OnTransform(ndFloat32, const ndMatrix& matrix)
{
	// apply this transformation matrix to the application user data.
	if (*m_entity)
	{
		if (m_parentBody)
		{
			const ndMatrix parentMatrix(m_bindMatrix * m_parentBody->GetMatrix());
			const ndMatrix localMatrix(matrix * parentMatrix.OrthoInverse());

			const ndQuaternion rot(localMatrix);
			m_transform = ndTransform(rot, localMatrix.m_posit);
		}
		else
		{
			m_transform = ndTransform(matrix * m_bindMatrix);
		}
	}

	if (!CheckInWorld(matrix))
	{
		RemoveBody();
	}
}

void ndDemoEntityNotify::RemoveBody()
{
	// check world bounds
	ndBody* const body = GetBody();
	ndPhysicsWorld* const world = m_manager->GetWorld();
	world->DefferedRemoveBody(body);
}
